#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包  
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <string>
#include <string.h>
#include <sstream>
#include <iostream>
#include <fstream> 
#include <vector>
#include <math.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Eigen>
#include <algorithm>
#include <fstream>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

using namespace std;
using namespace Eigen;

Vector3d box0_current_p(0,0,0);
Vector3d box1_current_p(0,0,0);
Vector3d box2_current_p(0,0,0);

void box0_cb(nav_msgs::Odometry msg)
{
	box0_current_p(0)=msg.pose.pose.position.x;
	box0_current_p(1)=msg.pose.pose.position.y;
	box0_current_p(2)=msg.pose.pose.position.z;
}

void box1_cb(nav_msgs::Odometry msg)
{
	box1_current_p(0)=msg.pose.pose.position.x;
	box1_current_p(1)=msg.pose.pose.position.y;
	box1_current_p(2)=msg.pose.pose.position.z;
}
void box2_cb(nav_msgs::Odometry msg)
{
	box2_current_p(0)=msg.pose.pose.position.x;
	box2_current_p(1)=msg.pose.pose.position.y;
	box2_current_p(2)=msg.pose.pose.position.z;
}

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "send_box_vel"); 

    ros::NodeHandle nh; 

    const int LOOPRATE = 20;
    ros::Rate loop_rate(LOOPRATE);
	double delt_t=1.0/LOOPRATE;
    ros::Publisher  box0_vel_pub = nh.advertise<geometry_msgs::Twist>("/box0/cmd_vel", 1);
    ros::Publisher  box1_vel_pub = nh.advertise<geometry_msgs::Twist>("/box1/cmd_vel", 1);
    ros::Publisher  box2_vel_pub = nh.advertise<geometry_msgs::Twist>("/box2/cmd_vel", 1);



    ros::Subscriber box0_sub = nh.subscribe("/box0/odom", 1, box0_cb);
    ros::Subscriber box1_sub = nh.subscribe("/box1/odom", 1, box1_cb);
    ros::Subscriber box2_sub = nh.subscribe("/box2/odom", 1, box2_cb);

///////////////
	Eigen::Vector3d box0_init_p(6,1.5,0);
	Eigen::Vector3d box0_final_p(-2,1.5,0);
	double box0_set_vel=2.0;
	double box0_time=(box0_final_p-box0_init_p).norm()/box0_set_vel;

	int box0_times=box0_time/delt_t;

	MatrixXd box0_p=Eigen::MatrixXd::Zero(2*box0_times,3);

	for(int i=0;i<box0_p.rows();i++)
	{
		if(i<=box0_times)
			box0_p.row(i)=(box0_final_p-box0_init_p)/box0_times*i+box0_init_p;
		else
			box0_p.row(i)=(box0_init_p-box0_final_p)/box0_times*(i-box0_times)+box0_final_p;
		ROS_INFO("BOX0_P ROW: %f %f %f",box0_p(i,0),box0_p(i,1),box0_p(i,2));
	}
	int box0_index=0;

///////////////////////////
	Eigen::Vector3d box1_init_p(6,4,0);
	Eigen::Vector3d box1_final_p(-1,-4,0);
	double box1_set_vel=3.0;
	double box1_time=(box1_final_p-box1_init_p).norm()/box1_set_vel;

	int box1_times=box1_time/delt_t;

	MatrixXd box1_p=Eigen::MatrixXd::Zero(2*box1_times,3);

	for(int i=0;i<box1_p.rows();i++)
	{
		if(i<=box1_times)
			box1_p.row(i)=(box1_final_p-box1_init_p)/box1_times*i+box1_init_p;
		else
			box1_p.row(i)=(box1_init_p-box1_final_p)/box1_times*(i-box1_times)+box1_final_p;
		ROS_INFO("BOX1_P ROW: %f %f %f",box1_p(i,0),box1_p(i,1),box1_p(i,2));
	}
	int box1_index=0;
////////////////////////////////
	Eigen::Vector3d box2_init_p(3,6,0);
	Eigen::Vector3d box2_final_p(3,-6,0);
	double box2_set_vel=1.0;
	double box2_time=(box2_final_p-box2_init_p).norm()/box2_set_vel;

	int box2_times=box2_time/delt_t;

	MatrixXd box2_p=Eigen::MatrixXd::Zero(2*box2_times,3);

	for(int i=0;i<box2_p.rows();i++)
	{
		if(i<=box2_times)
			box2_p.row(i)=(box2_final_p-box2_init_p)/box2_times*i+box2_init_p;
		else
			box2_p.row(i)=(box2_init_p-box2_final_p)/box2_times*(i-box2_times)+box2_final_p;
		ROS_INFO("BOX2_P ROW: %f %f %f",box2_p(i,0),box2_p(i,1),box2_p(i,2));
	}
	int box2_index=0;
///////////////////
	double kp=3;


    while(ros::ok)
    {

		if(box0_index==box0_p.rows()) box0_index=0;
		Eigen::Vector3d tmp=box0_p.row(box0_index);
		Eigen::Vector3d set_box0_v=(tmp-box0_current_p)*kp;

		geometry_msgs::Twist box0_set_v;
		box0_set_v.linear.x=set_box0_v(0);
		box0_set_v.linear.y=set_box0_v(1);
		box0_set_v.linear.z=0;

		box0_vel_pub.publish(box0_set_v);

		box0_index++;


		if(box1_index==box1_p.rows()) box1_index=0;
		tmp=box1_p.row(box1_index);
		Eigen::Vector3d set_box1_v=(tmp-box1_current_p)*kp;

		geometry_msgs::Twist box1_set_v;
		box1_set_v.linear.x=set_box1_v(0);
		box1_set_v.linear.y=set_box1_v(1);
		box1_set_v.linear.z=0;

		box1_vel_pub.publish(box1_set_v);

		box1_index++;



		if(box2_index==box2_p.rows()) box2_index=0;
		tmp=box2_p.row(box2_index);
		Eigen::Vector3d set_box2_v=(tmp-box2_current_p)*kp;

		geometry_msgs::Twist box2_set_v;
		box2_set_v.linear.x=set_box2_v(0);
		box2_set_v.linear.y=set_box2_v(1);
		box2_set_v.linear.z=0;

		box2_vel_pub.publish(box2_set_v);

		box2_index++;


		// ROS_INFO_ONCE("enter loop////");

		// geometry_msgs::PoseStamped goal_position;
		
		// goal_position.pose.position.x=goal_position_matrix[index](0);
		// goal_position.pose.position.y=goal_position_matrix[index](1);
		// goal_position.pose.position.z=goal_position_matrix[index](2);
		// goal_position.header.stamp=ros::Time::now();
        // disable_avoidance_goal_pub.publish(goal_position);

		// if((current_p-goal_position_matrix[index]).norm()<0.3)
		// {
		// 	index++;
		// 	if(index>=goal_position_matrix.size()-1)
		// 	{
		// 		index=goal_position_matrix.size()-1;
		// 	}
		// }
		ros::spinOnce();
		loop_rate.sleep();
	}


	return 0;

}